#include "stdafx.h"
#include "CollisionPrimitives.h"


void test()
{
	CollisionCone myCone;
}


bool CollisionRect::CollideRay(const mdsRay<double> & rayIn, mdsVector<3, double> vecResult) const
{
	if(rayIn.MinDistToPointSqrd(m_p3Center) > m_p3Size.Dot(m_p3Size))
	{
		return false;
	}
	vecResult = rayIn.m_p3Pos-m_p3Center;
	vecResult = m_matInverse.preMultVec(vecResult);
	mdsVector<3, double> p3Dir = m_matInverse.preMultVec(rayIn.m_p3Dir);

	int nDirBitvec = OctUtils::Vec3To3dQuadrant(p3Dir);

	double lfMaxNearPlane = -1.0f;
	double lfMinFarPlane = fabs(vecResult[0])+fabs(vecResult[1])+fabs(vecResult[2])
		+m_p3Size[0]+m_p3Size[1]+m_p3Size[2];

	mdsVector<3, double> vecFarCorner = m_p3Size;
	OctUtils::QuadrantToMdsVector(nDirBitvec, vecFarCorner);

	for(int nAxis = 0; nAxis < 3; ++nAxis)
	{
		// collide with slab along this axis
		if((-vecFarCorner[nAxis]-vecResult[nAxis])*p3Dir[nAxis] > 0.0)
		{
			double lfDistToGo = (-vecFarCorner[nAxis]-vecResult[nAxis])/p3Dir[nAxis];
			lfDistToGo += GenericMath::eps;
			vecResult[0] += lfDistToGo*p3Dir[0];
			vecResult[1] += lfDistToGo*p3Dir[1];
			vecResult[2] += lfDistToGo*p3Dir[2];
		}
	}

	if((fabs(vecResult[0]) < m_p3Size[0]) &&
			(fabs(vecResult[1]) < m_p3Size[1]) && 
			(fabs(vecResult[2]) < m_p3Size[2]))
	{
		vecResult = m_matInverse.preMultVec(vecResult);
		vecResult += m_p3Center;
		return true;
	}

	return false;
}

